<!DOCTYPE html>
<html>
<head>
<meta charset="utf-8" />
<style>
    html, body 
    {
        background-color: var(--vscode-editor-background);
        padding: 0;
        margin: 0;
        height: 100%;
        width: 100%;
        display: flex;
        align-items: stretch;
    }
    
    #urdf
    {
        width: 100%;
        height: 100%;
        overflow:hidden;
    }
</style>


<script src="https://static.robotwebtools.org/threejs/current/three.js"></script>
<script src="https://static.robotwebtools.org/threejs/current/ColladaLoader.js"></script>
<script src="https://static.robotwebtools.org/threejs/current/STLLoader.js"></script>
<script src="https://static.robotwebtools.org/EventEmitter2/current/eventemitter2.min.js"></script>
<script src="https://static.robotwebtools.org/roslibjs/current/roslib.js"></script>
<script src="https://static.robotwebtools.org/ros3djs/current/ros3d.js"></script>

<script>
    
function TFFakeClient() {
  this.callbacks = new Map();
}

TFFakeClient.prototype.subscribe = function(frameID, callback) {
    this.callbacks.set(frameID, callback);
}

TFFakeClient.prototype.unsubscribe = function(frameID, callback) {
    this.callbacks.delete(frameID);
}


function applyURDF(urdfText) {
    if (window.urdfScene) {
        window.viewer.scene.remove(window.urdfScene); 
    }

    window.tfCallback = new Map();

    window.urdfModel = new ROSLIB.UrdfModel({
        string : urdfText,
        path: "https:"
      });

    var tfClient = new TFFakeClient();

    window.urdfScene = new ROS3D.Urdf({
      urdfModel : window.urdfModel,
      path: "https:",
      tfClient : tfClient,
      loader : ROS3D.COLLADA_LOADER_2
    });
    
    window.viewer.scene.add(window.urdfScene);

    let joints = new Map();
    for(let jointName in window.urdfModel.joints) {
      let joint = window.urdfModel.joints[jointName];
      joints.set(joint.child, { 
        parent: joint.parent,
        origin: joint.origin
      });
    }

    // update tfClient callbacks to inject joint transform data
    for (let entry of tfClient.callbacks) {
      let frameID = entry[0];
      if (frameID[0] === '/') {
        frameID = frameID.substring(1);
      }

      if (joints.has(frameID)) {
        // callback expects a transform wrt the world.
        let initialPose = new ROSLIB.Pose();
        let tempFrameID = frameID;
        while (joints.has(tempFrameID)) {
          let origin = joints.get(tempFrameID).origin;
          let tf = new ROSLIB.Transform({translation : origin.position, rotation : origin.orientation});
          initialPose.applyTransform(tf);
          tempFrameID = joints.get(tempFrameID).parent;
        }
        let tf = new ROSLIB.Transform({translation : initialPose.position, rotation : initialPose.orientation});
        entry[1](tf);
      } else {
        let tf = new ROSLIB.Transform();
        entry[1](tf);
      }
    }
  }
  function init() {
    const vscode = acquireVsCodeApi();

    window.addEventListener('message', event => {
        const message = event.data; // The JSON data our extension sent
        switch (message.command) {
            case 'urdf':
            applyURDF(message.urdf);
            break;
            case 'previewFile':
            vscode.setState({previewFile: message.previewFile});
            break;
        }
      });    
      
      // Create the main viewer.
      window.viewer = new ROS3D.Viewer({
        divID : 'urdf',
        width : urdf.clientWidth,
        height : urdf.clientHeight,
        background : '#00000000',
        alpha : 0.001,
        antialias : true,
        intensity : 0.1
      });
      // Add a dominant hemisphere light for even lighting
      const hemiLight = new THREE.HemisphereLight(0xffffff, 0x444444, 0.8);
      hemiLight.position.set(0, 0, 10);
      window.viewer.addObject(hemiLight);

      // Add a grid.
      window.viewer.addObject(new ROS3D.Grid());
  }
  function resize() {
    if (window.viewer) {
        window.viewer.resize(urdf.clientWidth, urdf.clientHeight);
    }      
  }
</script>
</head>

<body onload="init()"  onresize="resize()">
  <div id="urdf"></div> 
</body>
</html>
